/*
 * @Author: Sian Li lisian_magic@163.com
 * @Date: 2024-07-05 14:07:21
 * @LastEditors: Sian Li lisian_magic@163.com
 * @LastEditTime: 2024-07-05 16:06:40
 * @FilePath: /hudar/src/hudar/src/RadarClass/GUI/gui.cpp
 * @Description:
 */
#include "../Radar/include/config.h"
#include "rclcpp/rclcpp.hpp"
#include "rm_msg/msg/location.hpp"
#include "rm_msg/msg/locations.hpp"
#include "std_msgs/msg/bool.hpp"
#include <cv_bridge/cv_bridge.h>
#include <memory> //使用智能指针
#include <opencv2/opencv.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <thread>
#include <vector>
static cv_bridge::CvImageConstPtr result_image;
static std::vector<rm_msg::msg::Location> locs;

using namespace cv;
class GUI : public rclcpp::Node {

  public:
    GUI() : Node("GUI") {
        this->declare_parameter("/gui/rmuc_map", "None");
        this->map_path = this->get_parameter("/gui/rmuc_map").as_string();
        if (this->map_path == "None") {
            RCLCPP_INFO(this->get_logger(), "Can't map_path");
            exit(-1);
        }
        // 订阅雷达节点数据
        this->sub_result = this->create_subscription<sensor_msgs::msg::Image>(
            "/radar/result_view", 10,
            std::bind(&GUI::image_msgCallback, this, std::placeholders::_1));

        this->sub_locs = this->create_subscription<rm_msg::msg::Locations>(
            "/radar/robots", 10,
            std::bind(&GUI::locations_msgCallback, this,
                      std::placeholders::_1));
        // 发布调试信号
        this->pub_cover =
            this->create_publisher<std_msgs::msg::Bool>("/gui/cover_depth", 10);
        this->pub_exit =
            this->create_publisher<std_msgs::msg::Bool>("/gui/exit", 10);
        // 读取地图

        if (!map_path.empty() && access(this->map_path.c_str(), F_OK) == 0) {
            map = cv::imread(this->map_path);
            if (ENEMY == 1)
                cv::flip(map, map, -1);
        } else {
            RCLCPP_INFO(this->get_logger(), "No Map Files Found");
        }
        // 创建窗体
        namedWindow("GUI", cv::WINDOW_GUI_NORMAL);
        createTrackbar("Exit Program", "GUI", 0, 1, nullptr);
        setTrackbarPos("Exit Program", "GUI", 0);
        createTrackbar("CoverDepth", "GUI", 0, 1, nullptr);
        setTrackbarPos("CoverDepth", "GUI", 0);

        this->guiProcess = std::thread(std::bind(&GUI::guiProcessLoop, this));
    }
    ~GUI() { RCLCPP_INFO(this->get_logger(), "GUI Destoried!"); }

  private:
    std::string map_path;
    std::thread guiProcess;
    bool if_exit_program = false;
    bool if_cover_depth = false;
    cv::Mat map;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_result;
    rclcpp::Subscription<rm_msg::msg::Locations>::SharedPtr sub_locs;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_exit;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_cover;
    void publishCoverDepth(std_msgs::msg::Bool msg) {}
    void guiProcessLoop() {
        RCLCPP_INFO(this->get_logger(), "GUI Process Loop Started!");
        while (!if_exit_program && rclcpp::ok()) {
            if (!result_image || result_image->image.empty())
                continue;
            Mat display = Mat::zeros(
                Size(result_image->image.cols + result_image->image.rows * 0.54,
                     result_image->image.rows),
                CV_8UC3);
            result_image->image.copyTo(display(Rect(
                0, 0, result_image->image.cols, result_image->image.rows)));
            if (!map.empty()) {
                cv::resize(map,
                           display(Rect(result_image->image.cols, 0,
                                        result_image->image.rows * 0.54,
                                        result_image->image.rows)),
                           Size(result_image->image.rows * 0.54,
                                result_image->image.rows));
            }
            for (auto it : locs) {
                if (it.x == 0 && it.y == 0)
                    continue;
                if (ENEMY == 1) {
                    it.x = 28.0 - it.x;
                    it.y = 15.0 - it.y;
                }
                Scalar color;
                std::string text;
                if (it.id < 6) {
                    color = Scalar(255, 0, 0);
                    text += "B";
                } else {
                    color = Scalar(0, 0, 255);
                    text += "R";
                }
                Point2f center =
                    Point2f((it.y / 15.0) * result_image->image.rows * 0.54 +
                                result_image->image.cols,
                            (it.x / 28.0) * result_image->image.rows);
                circle(display, center, 40.0, color, 3);
                text += std::to_string(it.id);
                int baseline;
                Size text_size =
                    getTextSize(text, FONT_HERSHEY_SIMPLEX, 1, 2, &baseline);
                center.x = center.x - text_size.width / 2;
                center.y = center.y + (text_size.height) / 2;
                putText(display, text, center, FONT_HERSHEY_SIMPLEX, 1, color,
                        2);
            }
            cv::imshow("GUI", display);
            cv::resizeWindow("GUI", Size(1536, 864));
            bool if_exit_program_current =
                getTrackbarPos("Exit Program", "GUI") == 1 ? true : false;
            bool if_coverDepth_current =
                getTrackbarPos("CoverDepth", "GUI") == 1 ? true : false;

            if (if_exit_program_current != if_exit_program) {
                if_exit_program = if_exit_program_current;
                std_msgs::msg::Bool msg;
                msg.data = if_exit_program;
                this->pub_exit->publish(msg);
            }
            if (if_coverDepth_current != if_cover_depth) {
                if_cover_depth = if_coverDepth_current;
                std_msgs::msg::Bool msg;
                msg.data = if_cover_depth;
                this->pub_cover->publish(msg);
            }
            cv::waitKey(1);
        }
        RCLCPP_INFO(this->get_logger(), "GUI Process Loop Shutdown!");
        cv::destroyWindow("GUI");
        rclcpp::shutdown();
    }
    void image_msgCallback(const sensor_msgs::msg::Image::SharedPtr msg) {
        try {
            result_image =
                cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
        } catch (cv_bridge::Exception &e) {
            RCLCPP_INFO(this->get_logger(), "Subscribe Image Failed!");
            return;
        }
    }

    void locations_msgCallback(const rm_msg::msg::Locations::ConstPtr &msg) {
        locs.clear();
        for (auto &it : msg->locs) {
            locs.emplace_back(it);
        }
    }
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<GUI>();
    rclcpp::spin(node);

    rclcpp::shutdown();
    return 0;
}